
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       rc_do_function.c
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <stdint.h>
#include <stdbool.h>

#include "rc_channel.h"

#include <common/gp_defines.h>
#include <mavproxy/mavproxy.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static bool (*flight_task_set_mode_cb)(int8_t mode, int8_t reason);
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void RC_register_callback(bool (*cb)(int8_t mode, int8_t reason))
{
    flight_task_set_mode_cb = cb;
}

void RC_mode_switch_changed(RC_HandleTypeDef *pRc, modeswitch_pos_t new_pos)
{
    if (new_pos < 0 || (uint8_t)new_pos > 2) {
        // should not have been called
        return;
    }

    if (flight_task_set_mode_cb == NULL) {
        return;
    }

    if (!flight_task_set_mode_cb(new_pos, MODE_REASON_RC_COMMAND)) {
        // alert user to mode change failure
        //TODO:
        return;
    }
    
    // play a tone
    // alert user to mode change (except if autopilot is just starting up)
    //TODO:
    
}

void RC_init_aux(RC_HandleTypeDef *pRc)
{
    RC_AuxSwitchPos position;
    if (!RC_read_3pos_switch(pRc, &position)) {
        position = RC_LOW;
    }
    RC_init_aux_function(pRc, (RC_AuxFunc_t)pRc->_option, position);
}

bool RC_read_aux(RC_HandleTypeDef *pRc)
{
    const RC_AuxFunc_t _option = (RC_AuxFunc_t)pRc->_option;
    if (_option == RC_DO_NOTHING) {
        // may wish to add special cases for other "AUXSW" things
        // here e.g. RCMAP_ROLL etc once they become options
        return false;
    }
    RC_AuxSwitchPos new_position;
    if (!RC_read_3pos_switch(pRc, &new_position)) {
        return false;
    }

    if (!RC_debounce_completed(pRc, (int8_t)new_position)) {
        return false;
    }

    // debounced; undertake the action:
    RC_do_aux_function(pRc, _option, new_position);
    return true;
}

// init_aux_switch_function - initialize aux functions
void RC_init_aux_function(RC_HandleTypeDef *pRc, const RC_AuxFunc_t ch_option, const RC_AuxSwitchPos ch_flag)
{
    // init channel options
    switch(ch_option) {
    // the following functions do not need to be initialised:
    case RC_DISARM:
        break;
    case RC_FENCE:
        RC_do_aux_function(pRc, ch_option, ch_flag);
        break;
    default:
        mavproxy_send_statustext(MAV_SEVERITY_WARNING, "Failed to init: RC%u_OPTION: %u\n",
                           (unsigned)(pRc->ch_in+1), (unsigned)ch_option);
        break;
    }
}

/**
  * @brief       
  * @param[in]   ch_option  
  * @param[in]   ch_flag  
  * @param[out]  
  * @retval      
  * @note        
  */
void RC_do_aux_function(RC_HandleTypeDef *pRc, const RC_AuxFunc_t ch_option, const RC_AuxSwitchPos ch_flag)
{
    switch(ch_option) {
    case RC_DISARM:
        if (ch_flag == RC_HIGH) {
            //FlightTask_InitDisarmMotors();
        }
        break;

    default:
        mavproxy_send_statustext(MAV_SEVERITY_INFO, "Invalid channel option (%u)", (unsigned int)ch_option);
        break;
    }
}

/*------------------------------------test------------------------------------*/


